#include "Car_port.h"

// import
#include "main.h"
#include "board_base.h"
#include "Motor.h"
#include "mpu6050.h"
#include "Car.h"
#include "ws_config.h"

#include "Car_ctrl.h"
// [0,1]
void bsp_led_write(int id, uint8_t state)
{
    if (state)
        RGB_SET(0, 0xFF,0xFF,0xFF);
    else
        RGB_SET(0, 00, 00, 00);
}
// [0,1]
void bsp_buzzer_write(uint8_t state)
{
    HAL_GPIO_WritePin(Buzzer_GPIO_Port, Buzzer_Pin,
                      state ? GPIO_PIN_RESET : GPIO_PIN_SET);
}
// [-100,100] 左轮占空比和方向
void bsp_left_pwm_write(float duty)
{
    MotorL_Set_Rate(duty*100);
}
// [-100,100] 右轮占空比和方向
void bsp_right_pwm_write(float duty)
{
    MotorR_Set_Rate(duty*100);
}
// [0,1] 电机使能
void bsp_motor_enable(uint8_t state)
{
    if (state)
    {
        MotorL_Start();
        MotorR_Start();
    }
    else
    {
        MotorL_Stop();
        MotorR_Stop();
    }

}
// 编码器值
int bsp_encoder_read(int id)
{
	switch (id)
	{
		case 0:
			return MotorL.encoder;
		case 1:
			return MotorR.encoder;
	}
	return 0;
}

void bsp_encoder_clear(int id)
{
	switch (id)
	{
		case 0:
			MotorL.encoder = 0;
            break;
		case 1:
			MotorR.encoder = 0;
            break;
	}
}

// 电子罗盘
float bsp_angle_read(void)
{
    return car.angle;
}
// z轴角速度
float bsp_gyroz_read(void)
{
    return mpu6050.Gyro_Z;
}
// 循迹模块i=0为右
int bsp_tracker_read(uint8_t *buf, int size)
{
    uint16_t temp = traceData.data;
    if (size == 16)
    {
        for (int i = 0; i < 16; i++)
        {
            buf[i] = temp&1;
            temp>>=1;
        }
        return 16;
    }
    if (size == 8)
    {
        // 中间八位
        for (int i = 4; i < 12; i++)
        {
            buf[i-4] = temp&1;
            temp>>=1;
        }
        return 8;
    }
    return 0;
}
// flash写入
int bsp_flash_write(void* data, int size)
{
    WS_Config_Write_Struct_Callback((uint8_t *)data, size);
	return size;
}
// flash读出
int bsp_flash_read(void* buf, int size)
{
    WS_Config_Read_Struct_Callback((uint8_t *)buf, size);
	return size;
}
// 电池电压
float bsp_power_read(void)
{
    return car.powerV;
}

CarPort_t CarPort =
    {
        .led_write = bsp_led_write,             // [0,1]
        .buzzer_write = bsp_buzzer_write,       // [0,1]
        .left_pwm_write = bsp_left_pwm_write,   // [-100,100] 左轮占空比和方向
        .right_pwm_write = bsp_right_pwm_write, // [-100,100] 右轮占空比和方向
        .motor_enable = bsp_motor_enable,       // [0,1] 电机使能
        .encoder_read = bsp_encoder_read,       // 编码器值
		.encoder_clear = bsp_encoder_clear,     // 编码器清零
        .angle_read = bsp_angle_read,           // 电子罗盘
        .gyroz_read = bsp_gyroz_read,           // z轴加速度
		.tracker_read = bsp_tracker_read,       // 循迹模块i=0为右
        .flash_write = bsp_flash_write,         // flash写入
        .flash_read = bsp_flash_read,           // flash读出
        .power_read = bsp_power_read,           // 电池电压
};

void CarPort_Init(void)
{
    Motor_Init();
    Car_Init();
    car_ctrl_init(&CarPort);
    // TODO:
}


